import traceback

import matplotlib.pyplot as plt
import numpy as np

import math
import time

from Common.ControlEnum import forcemode
from Hardware.UrRobot import UrRobot
from Hardware import FT300
from Common import INIFile
from Hardware import FTur5
from Algorithm.AlgorithmModel import ControlResult
from Algorithm.M_my_test import my_test



def main():
    # region 1,初始化机器人、力传感器
    # region 获取配置文件
    a2r = math.pi / 180
    config = INIFile.IniConfig('config.ini')
    robotip = config.read('hardware', 'robotip')
    useFt = config.read('hardware', 'UseForceSensor')
    robot_type=config.read('hardware', 'robot')
    teachMode=forcemode(int(config.read('hardware', 'Teachmode')))
    dt=0.008 if robot_type=="ur5" else 0.002
    print(robotip,useFt,dt,teachMode)
    # endregion

    # region 硬件初始化
    ur5 = UrRobot(robotip=robotip, forcesensor=FT300.RobotiqFt300() if useFt == "True" else FTur5.Ftur5(),dt=dt)
    ur5.init()
    # endregion
    initpose=[-24.2 * a2r, -116.30 * a2r, -63.68 * a2r, -88.76 * a2r, -269.23 * a2r, -27.81 * a2r]
    initpose=[-26.65 * a2r, -106.76 * a2r, -79.07 * a2r, -82.91 * a2r, -269.2 * a2r, -30.15 * a2r]

    ur5.moveJ(initpose, 0.8, 0.1)
    time.sleep(1)
    initPose = ur5.getActualTCPPose()
    # print(cv2.Rodrigues(np.array([2.1,2.3,0.5]))[0])
    print(initPose)
    ur5.FtZero()
    # 3  test轨迹
    target_v = 0.01

    initPose = ur5.getActualTCPPose()
    algorithm = my_test(initPose,dt,teachMode)
    ur5.setControlFun(algorithm.Move)
    ur5.setStartRealTimeControl(True)
    ur5.enableLog()

    while (True):
        time.sleep(0.01)
        if (ur5.getRealTimeControlResult() == ControlResult.Finish):
            break
    # endregion


    data = ur5.disableLog()
    time.sleep(1)
    #桌面硬表面测试
    #ur5.moveJ([-258.8 * a2r, -131.33 * a2r, -63.38 * a2r, -50.85 * a2r, 85.1 * a2r, -41.07 * a2r], 0.8, 0.1)
    count = len(data)
    plt.figure(1)
    # plt.axis("equal")
    posx = []
    posy = []
    posz = []
    fz=[]
    fx=[]
    fy=[]
    vel = []
    for i in range(count):
        posx.append(data[i]['actualTcpPose'][0])
        posy.append(data[i]['actualTcpPose'][1])
        posz.append(data[i]['actualTcpPose'][2])
        fz.append(data[i]['force'][2])
        fx.append(data[i]['force'][0])
        fy.append(data[i]['force'][1])
        vel.append(np.linalg.norm(data[i]['actualTcpSpeed'][1]))
    # plt.plot(posx,posy)
    np.savetxt("force.txt",fz)
    np.savetxt("pos.txt",posz)
    axisx = range(1, len(vel) + 1)
    # plt.figure(2)
    plt.plot(axisx, fz)
    plt.plot(axisx,fx)
    plt.plot(axisx,fy)
    plt.show()

    plt.figure(2)
    plt.plot(axisx, posz)
    plt.show()
    ur5.disconnect()


if __name__ == '__main__':
    main()
